# coding=utf-8
import time
import pyrealsense2 as rs
import numpy as np
import cv2
import threading


W = 640
H = 480

'''
求取图像上一个点的三维坐标
'''
def get_3d_point(depth_frame, xy):
    x, y = int(xy[0]), int(xy[1])
    # 获取像素点的深度值
    depth = depth_frame.get_distance(x, y)
    # 如果是噪点，则用四周不为0的点的深度值的平均值代替
    if depth < 0.01:
        count = 0
        for i in range(-3, 4):
            for j in range(-3, 4):
                # 防止越界
                x_limit = max(min(x+i, W-1), 0)
                y_limit = max(min(y+j, H-1), 0)
                if depth_frame.get_distance(x_limit, y_limit) > 0.01:
                    depth += depth_frame.get_distance(x_limit, y_limit)
                    count += 1
        if count != 0:
            depth = depth / count
    if depth == 0:
        print("depth is 0")
    # intelrealsense参数
    intrinsics = depth_frame.profile.as_video_stream_profile().intrinsics
    # 像素坐标系转换为相机坐标系
    camera_coordinate = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], depth)
    return camera_coordinate

'''
三点坐标 -> 正方体前表面坐标，正方体三个互相垂直面的单位法向量
'''
def getCenter3(p1, p2, p3):
    p1 = np.array(p1)
    p2 = np.array(p2)
    p3 = np.array(p3)
    # 化p1,p3在同一条对角线上
    p1p2 = np.linalg.norm(p1 - p2)
    p1p3 = np.linalg.norm(p1 - p3)
    p2p3 = np.linalg.norm(p2 - p3)
    if(p1p2 > p1p3 and p1p2 > p2p3):
        p3, p2 = p2, p3
    elif(p2p3 > p1p2 and p2p3 > p1p3):
        p1, p2 = p2, p1
    # p1p2向量(一个法向量)
    vec2 = np.array(p2) - np.array(p1)
    # p2p3向量(另一个法向量)
    vec3 = np.array(p3) - np.array(p2)
    # 正面法向量
    vec1 = np.cross(vec2, vec3)
    # 法向量单位化
    vec_norm1 = np.linalg.norm(vec1)
    unit_vec1 = vec1 / (vec_norm1 if vec_norm1 != 0 else 1)
    vec_norm2 = np.linalg.norm(vec2)
    unit_vec2 = vec2 / (vec_norm2 if vec_norm2 != 0 else 1)
    vec_norm3 = np.linalg.norm(vec3)
    unit_vec3 = vec3 / (vec_norm3 if vec_norm3 != 0 else 1)
    
    # z轴法向量指向正方向
    if(unit_vec1[2] < 0):
        unit_vec1 = -unit_vec1
    # 平面中心
    front_center = (p1 + p3) / 2
    # 三个单位法向量
    vec_z = unit_vec1
    if unit_vec2[0] < 0:
        unit_vec2 = -unit_vec2
    if unit_vec3[0] < 0:
        unit_vec3 = -unit_vec3
    if np.dot(unit_vec2, np.array([1, 0, 0])) > np.dot(unit_vec3, np.array([1, 0, 0])):
        vec_x = unit_vec2
        vec_y = unit_vec3
    else:
        vec_x = unit_vec3
        vec_y = unit_vec2
    if vec_x[0] < 0:
        vec_x = -vec_x
    if vec_y[1] < 0:
        vec_y = -vec_y
    # 兑换站中心、前方向、下方向、右方向
    return front_center, vec_z, vec_x, vec_y

def getCenter4(p1, p2, p3, p4):

    p1, p2, p3, p4 = np.array(p1), np.array(p2), np.array(p3), np.array(p4)
    points_3d = [p1, p2, p3, p4]
    points_3d = np.array(points_3d)
    front_center = [0, 0, 0]
    front_center[0] = (points_3d[0][0] + points_3d[1][0] + points_3d[2][0] + points_3d[3][0]) / 4
    front_center[1] = (points_3d[0][1] + points_3d[1][1] + points_3d[2][1] + points_3d[3][1]) / 4
    front_center[2] = (points_3d[0][2] + points_3d[1][2] + points_3d[2][2] + points_3d[3][2]) / 4

    vec2 = p1 + p2 - p3 - p4
    vec3 = p2 + p3 - p1 - p4
    vec1 = np.cross(vec2, vec3)

    vec_norm1 = np.linalg.norm(vec1)
    unit_vec1 = vec1 / (vec_norm1 if vec_norm1 != 0 else 1)
    vec_norm2 = np.linalg.norm(vec2)
    unit_vec2 = vec2 / (vec_norm2 if vec_norm2 != 0 else 1)
    vec_norm3 = np.linalg.norm(vec3)
    unit_vec3 = vec3 / (vec_norm3 if vec_norm3 != 0 else 1)

    # 三个单位法向量
    vec_z = unit_vec1 if unit_vec1[2] > 0 else -unit_vec1
    if unit_vec2[0] < 0:
        unit_vec2 = -unit_vec2
    if unit_vec3[0] < 0:
        unit_vec3 = -unit_vec3
    if np.dot(unit_vec2, np.array([1, 0, 0])) > np.dot(unit_vec3, np.array([1, 0, 0])):
        vec_x = unit_vec2
        vec_y = unit_vec3
    else:
        vec_x = unit_vec3
        vec_y = unit_vec2
    if vec_x[0] < 0:
        vec_x = -vec_x
    if vec_y[1] < 0:
        vec_y = -vec_y
    # 兑换站中心、前方向、下方向、右方向
    return front_center, vec_z, vec_x, vec_y


'''
输出结果
'''
def printResult(color_image, depth_frame, points, points_num):
    global info
    points_num = len(points)
    if points_num == 4:
        # points = np.array(points)
        # center = np.mean(points, axis=0)
        center = [0, 0]
        for i in range(len(points)):
            center[0] += points[i][0]
            center[1] += points[i][1]
        center = np.array(center)
        center = np.int0(center / len(points))
        points.sort(key=(lambda p: np.arctan2(p[1] - center[1], p[0] - center[0])))
        cv2.line(color_image, points[0], points[2], (0, 255, 255), 2)
        cv2.line(color_image, points[1], points[3], (0, 255, 255), 2)
        for j in range(len(points)):
            cv2.circle(color_image, (points[j][0], points[j][1]), 4, (255, 255, 255), -1)
        points_3d = []
        for i in range(4):
            dep = get_3d_point(depth_frame, points[i])
            points_3d.append(dep)
        output = getCenter4(points_3d[0], points_3d[1], points_3d[2], points_3d[3])
    else:
        # 绘制points中的点
        for j in range(len(points)):
            cv2.circle(color_image, (points[j][0], points[j][1]), 4, (255, 255, 255), -1)
        # 读取points中的点的三维坐标
        points_3d = []
        for i in range(3):
            dep = get_3d_point(depth_frame, points[i])
            points_3d.append(dep)
        # 输出结果
        if len(points_3d) >= 3:
            output = getCenter3(points_3d[0], points_3d[1], points_3d[2])

    print("")
    print("center: ", output[0])
    print("front_vector: ", output[1])
    print("right_vector: ", output[2])
    return output[0], output[1], output[2]

'''
两点距离
'''
def getDistance(p1, p2):
    p1 = np.array(p1)
    p2 = np.array(p2)
    return np.linalg.norm(p1 - p2)

'''
获取轮廓内的所有点
'''
def points_in_contour(contour):
    inside = []
    rect = cv2.boundingRect(contour) 
    # 取boundingrect内的所有像素
    for m in range(rect[0], rect[0] + rect[2], 2):
        for n in range(rect[1], rect[1] + rect[3], 2):
            # 判断点是否在旋转矩形内
            if cv2.pointPolygonTest(contour, (m, n), False) > 0:
            # if binary[n][m] > 0:
                inside.append([m, n])
    return inside

'''
获取L型轮廓的几个三个角点、直角点、斜边中点
'''
def find_coutour_points(contour, color_image):
    points = []  
    time1 = time.time()          
    # 轮廓中最远的两个点
    rect = cv2.minAreaRect(contour)
    box = cv2.boxPoints(rect)
    box = np.int0(box)
    min_l_4 = []
    for i in range(len(box)):
        min_l = 100000000
        min_idx = 0
        for j in range(len(contour)):
            l = (box[i][0] - contour[j][0][0]) ** 2 + \
                (box[i][1] - contour[j][0][1]) ** 2
            if l < min_l:
                min_l = l
                min_idx = j
        min_l_4.append(min_idx)
    max_idx = [0, 0]
    max_l = 0
    for i in range(4):
        for j in range(i + 1, 4):
            l = (contour[min_l_4[i]][0][0] - contour[min_l_4[j]][0][0]) ** 2 + \
                (contour[min_l_4[i]][0][1] - contour[min_l_4[j]][0][1]) ** 2
            if l > max_l:
                max_l = l
                max_idx = [min_l_4[i], min_l_4[j]]
    
    points.append(contour[max_idx[0]][0])
    points.append(contour[max_idx[1]][0])
    # 轮廓中距离这两个点距离之和最远的点
    max_l = 0
    max_idx = 0
    for j in range(len(contour)):
        l = (contour[j][0][0] - contour[max_idx][0][0]) ** 2 + \
            (contour[j][0][1] - contour[max_idx][0][1]) ** 2
        l = getDistance(points[0], contour[j][0]) + \
            getDistance(points[1], contour[j][0])
        if l > max_l:  
            max_l = l
            max_idx = j
    points.append(contour[max_idx][0])

    for p in points:
        cv2.circle(color_image, p, 5, (255, 255, 0), -1)

    # num = [0, 0, 0]
    # for j in range(3):
    #     for k in range(j + 1, 3):
    #         center_2 = (np.array(points[j]) + np.array(points[k])) / 2
    #         center_2 = center_2.astype(np.int32)
    #         for x in range(center_2[0] - 3, center_2[0] + 3):
    #             for y in range(center_2[1] - 3, center_2[1] + 3):
    #                 if cv2.pointPolygonTest(contour, (x, y), False) > 0:
    #                     num[3 - j - k] += 1
    # point_90 = points[num.index(min(num))]

    l = [0, 0, 0]
    for j in range(3):
        for k in range(j + 1, 3):
            l[3  - i - j] = getDistance(np.array(points[j]), np.array(points[k]))
    max_l = 0
    max_idx = 0
    for i in range(3):
        if l[i] > max_l:
            max_idx = i
    point_90 = points[max_idx]
    
    
    
    # cv2.circle(color_image, (point_90[0], point_90[1]), 4, (255, 255, 0), -1)
    # 斜边中点
    center = np.array([0, 0])
    for j in range(3):
        if points[j][0] != point_90[0] or points[j][1] != point_90[1]:
            center += np.array(points[j])
    center = center / 2 # 除以2后为浮点数
    # cv2.circle(color_image, (int(center[0]), int(center[1])), 4, (255, 0, 255), -1)
    return points, point_90, center

'''
L形轮廓的两边交点
'''
def contour_point(contour, color_image):
    points, point_90, center = find_coutour_points(contour, color_image)
    cv2.circle(color_image, point_90, 5, (255, 255, 0), -1)
    # 轮廓分割(取半部分)
    # 可以画线来分
    center_line = np.array(center) - np.array(point_90)
    # center = np.array(center)
    # point_90 = np.array(point_90)
    center = np.int0(center)
    point_90 = np.int0(point_90)
    # cv2.line(color_image, center, point_90, (255, 0, 255), 2)
    count = len(contour)
    right = []
    left = []
    points_inside = points_in_contour(contour)
    for j in range(len(points_inside)):
        vector = np.array(points_inside[j]) - np.array(point_90)
        if np.cross(center_line, vector) > 0:
            right.append(points_inside[j])
        else:
            left.append(points_inside[j])
    right = np.array(right)
    right = right.astype(int)
    left = np.array(left)
    left = left.astype(int)
    if len(right) == 0 or len(left) == 0:
        print("contour_point: len(right) == 0 or len(left) == 0")
        return None
    right_rect = cv2.minAreaRect(right)
    left_rect = cv2.minAreaRect(left)
    box1 = cv2.boxPoints(right_rect)
    box2 = cv2.boxPoints(left_rect)
    box1 = np.int0(box1)
    box2 = np.int0(box2)
    # try:
    #     print("...........")
    #     # cv2.drawContours(color_image, [box1], 0, (255, 255, 0), 2)
    #     # cv2.drawContours(color_image, [box2], 0, (255, 255, 0), 2)
    # except:
    #     pass
    
    # 取旋转矩形内的点用来拟合直线
    # 右
    right = []
    rect = cv2.boundingRect(box1)
    # 取boundingrect内的所有像素
    for j in range(rect[0], rect[0] + rect[2]):
        for k in range(rect[1], rect[1] + rect[3]):
            # 判断点是否在contour内
            if cv2.pointPolygonTest(contour, (j, k), False) > 0:
                right.append([j, k])
    # for i in range(len(right)):
    #     cv2.circle(color_image, right[i], 1, (255, 255, 0), -1)
    # 将right转换为int
    right = np.array(right)
    right = right.astype(int)
    if len(right) < 2:
        print("right line error")
        return None
    # 左
    left = []
    rect = cv2.boundingRect(box2)
    for j in range(rect[0], rect[0] + rect[2]):
        for k in range(rect[1], rect[1] + rect[3]):
            # 判断点是否在contour内
            if cv2.pointPolygonTest(contour, (j, k), False) > 0:
                left.append([j, k])
    if len(left) == 0:
        print("left is none")
        return None
    # for i in range(len(left)):
    #     cv2.circle(color_image, left[i], 1, (255, 255, 0), -1)
    right_line = cv2.fitLine(right, cv2.DIST_L2, 0, 0.01, 0.01)
    # cv2.line(color_image, (int(right_line[2] - right_line[0] * 100), int(right_line[3] - right_line[1] * 100)), (int(right_line[2] + right_line[0] * 100), int(right_line[3] + right_line[1] * 100)), (0, 255, 255), 2)
    right_vec = np.array([right_line[0][0] , right_line[1][0]])
    right_vec = right_vec / (np.linalg.norm(right_vec) if np.linalg.norm(right_vec) != 0 else 1)
    if np.cross(center_line, right_vec) < 0:
        right_vec = -right_vec
    left_line = cv2.fitLine(np.array(left), cv2.DIST_L2, 0, 0.01, 0.01)
    # cv2.line(color_image, (int(left_line[2] - left_line[0] * 100), int(left_line[3] - left_line[1] * 100)), (int(left_line[2] + left_line[0] * 100), int(left_line[3] + left_line[1] * 100)), (0, 255, 255), 2)
    left_vec = np.array([left_line[0][0] , left_line[1][0]])
    left_vec = left_vec / (np.linalg.norm(left_vec) if np.linalg.norm(left_vec) != 0 else 1)
    if np.cross(center_line, left_vec) > 0:
        left_vec = -left_vec
    # 求right_line和left_line的交点
    a1, b1, c1 = right_line[1], -right_line[0], -right_line[1] * right_line[2] + right_line[0] * right_line[3]
    a2, b2, c2 = left_line[1], -left_line[0], -left_line[1] * left_line[2] + left_line[0] * left_line[3]
    x = (b1 * c2 - b2 * c1) /( (a1 * b2 - a2 * b1) if (a1 * b2 - a2 * b1) != 0 else 1)
    y = (a2 * c1 - a1 * c2) / ((a1 * b2 - a2 * b1) if (a1 * b2 - a2 * b1) != 0 else 1)
    return int(x), int(y)



# def simple_full_screen(img, window_name):
#     cv2.namedWindow(window_name, cv2.WND_PROP_FULLSCREEN)
#     cv2.moveWindow(window_name, 0,0)   
#     cv2.setWindowProperty(window_name,cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
#     cv2.imshow(window_name, img)
#     cv2.waitKey(0)

